#include "car.h"

#include <math.h>

#include "buz.h"
#include "enc.h"
#include "err.h"
#include "hc05.h"
#include "mpu.h"
#include "oled.h"
#include "pid.h"
#include "tim.h"
#include "usart.h"

int Car_Forward(Enc_HandleTypeDef henc, Mpu_HandleTypeDef hmpu,
                float distance) {
  uint32_t ti = 50;
  uint32_t time, last_time = 0, arrival_time = 0;
  Pid_HandleTypeDef hpid_pos, hpid_val, hpid_yaw;
  float speed;
  Enc_SetPos(&henc, 0, 0);                           // 路程归零
  Pid_Init(&hpid_pos, ti, 1, 1, 0, 10, 50, 0, 0.2);  // 需调参
  Pid_Init(&hpid_val, ti, 1, 1, 0, 0.1, 1, 0, 0.2);  // 需调参
  Pid_Init(&hpid_yaw, ti, 0.01, 0.0005, 0.02, 0.2, 1, 0, 0.3);
  while (1) {
    time = HAL_GetTick();
    if (arrival_time && time - arrival_time > 100) break;
    if (time - last_time > ti) {
      last_time = time;
      Enc_Update(&henc);
      Mpu_Update(&hmpu);
      speed = Pid_Calc(&hpid_val, Pid_Calc(&hpid_pos, henc.val));
      Enc_SetSpeed(&henc, speed, speed, Pid_Calc(&hpid_yaw, hmpu.yaw));
      Oled_ShowNumSmall(1, 30, (int16_t)(henc.pos1), 8);
      Oled_ShowNumSmall(2, 30, (int16_t)(henc.pos2), 8);
      Oled_ShowNumSmall(3, 24, (int16_t)(hmpu.yaw), 5);
    }
    if (henc.pos1 - distance < 1000 && henc.pos1 - distance > -1000)  // 需调参
      arrival_time = time;
  }
  return 0;
}

int Car_Turn(Enc_HandleTypeDef henc, Mpu_HandleTypeDef hmpu, float angle) {
  uint32_t ti = 50;
  uint32_t time, last_time = 0, arrival_time = 0;
  Pid_HandleTypeDef hpid_yaw;
  float error;
  int dir;  // 判断左转还是右转
  Mpu_Update(&hmpu);
  if ((angle - hmpu.yaw > 0 && angle - hmpu.yaw < 180) ||
      (angle - hmpu.yaw < -180))  // 左转
    dir = 1;
  else  // 右转
    dir = 0;

  Pid_Init(&hpid_yaw, ti, 0.08, 0.0005, 0.05, 0.2, 10, 0, 0.3);
  while (1) {
    time = HAL_GetTick();
    if (arrival_time && time - arrival_time > 100) break;
    if (time - last_time > ti) {
      last_time = time;
      Enc_Update(&henc);
      Mpu_Update(&hmpu);
      if (dir)
        error =
            hmpu.yaw - angle > 0 ? hmpu.yaw - angle : hmpu.yaw - angle + 360;
      else
        error =
            hmpu.yaw - angle < 0 ? hmpu.yaw - angle : hmpu.yaw - angle - 360;
      Enc_SetSpeed(&henc, 0, 0, Pid_Calc(&hpid_yaw, error));
      Oled_ShowNumSmall(1, 30, (int16_t)(henc.pos1), 8);
      Oled_ShowNumSmall(2, 30, (int16_t)(henc.pos2), 8);
      Oled_ShowNumSmall(3, 24, (int16_t)(hmpu.yaw), 5);
    }
    if (error < 5 && error > -5)  // 需调参
      arrival_time = time;
  }
  return 0;
}

int Car_main(void) {
  Enc_HandleTypeDef henc;
  Mpu_HandleTypeDef hmpu;
  Hc05_HandleTypeDef hch05;
  char cmd = '\0';
  Err_Init();
  Err_On();
  if (Oled_Init()) Err_Exit(1);
  if (Buz_Init()) Err_Exit(1);
  if (Enc_Init(&henc)) Err_Exit(2);
  if (Mpu_Init(&hmpu)) Err_Exit(3);
  if (Hc05_Init(&hch05)) Err_Exit(4);
  HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_13);
  HAL_Delay(1000);
  HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_13);
  Err_Off();

  Oled_ShowStringSmall(0, 0, "cmd:");
  Oled_ShowStringSmall(1, 0, "pos1:");
  Oled_ShowStringSmall(2, 0, "pos2:");
  Oled_ShowStringSmall(3, 0, "yaw:");
  float unit = 10000;  // 1米的count值，需调参
  while (1) {
    if (Hc05_Empty(&hch05)) continue;
    cmd = Hc05_Read(&hch05);
    Oled_ShowCharSmall(0, 24, cmd);
    switch (cmd) {
      case '1':  // 路线一
        Car_Forward(henc, hmpu, unit);
        Car_Turn(henc, hmpu, -60);
        Car_Forward(henc, hmpu, unit);
        Car_Turn(henc, hmpu, -120);
        Car_Forward(henc, hmpu, unit);
        Car_Turn(henc, hmpu, 180);
        Car_Forward(henc, hmpu, unit);
        Car_Turn(henc, hmpu, 240);
        Car_Forward(henc, hmpu, unit);
        Car_Turn(henc, hmpu, 300);
        Car_Forward(henc, hmpu, unit);
        break;
      case '2':  // 路线二
        Car_Forward(henc, hmpu, unit);
        Car_Turn(henc, hmpu, -60);
        Car_Forward(henc, hmpu, unit);
        Car_Turn(henc, hmpu, 180);
        Car_Forward(henc, hmpu, 2 * unit);
        Car_Turn(henc, hmpu, -60);
        Car_Forward(henc, hmpu, unit);
        Car_Turn(henc, hmpu, 0);
        Car_Forward(henc, hmpu, unit);
        Car_Turn(henc, hmpu, 120);
        Car_Forward(henc, hmpu, 20000);
        break;
      case '3':  // 路线三，即由A走到G
        Car_Turn(henc, hmpu, -60);
        Car_Forward(henc, hmpu, unit);
        Car_Turn(henc, hmpu, 0);
        break;
      case 'A':
        Car_Turn(henc, hmpu, 120);
        Car_Forward(henc, hmpu, unit);
        Car_Turn(henc, hmpu, -60);
        Car_Forward(henc, hmpu, unit);
        break;
      case 'B':
        Car_Turn(henc, hmpu, 60);
        Car_Forward(henc, hmpu, unit);
        Car_Turn(henc, hmpu, -120);
        Car_Forward(henc, hmpu, unit);
        break;
      case 'C':
        Car_Turn(henc, hmpu, 0);
        Car_Forward(henc, hmpu, unit);
        Car_Turn(henc, hmpu, 180);
        Car_Forward(henc, hmpu, unit);
        break;
      case 'D':
        Car_Turn(henc, hmpu, -60);
        Car_Forward(henc, hmpu, unit);
        Car_Turn(henc, hmpu, 120);
        Car_Forward(henc, hmpu, unit);
        break;
      case 'E':
        Car_Turn(henc, hmpu, -120);
        Car_Forward(henc, hmpu, unit);
        Car_Turn(henc, hmpu, 60);
        Car_Forward(henc, hmpu, unit);
        break;
      case 'F':
        Car_Turn(henc, hmpu, 180);
        Car_Forward(henc, hmpu, unit);
        Car_Turn(henc, hmpu, 0);
        Car_Forward(henc, hmpu, unit);
        Car_Turn(henc, hmpu, 0);
      default:
        // 未知指令通通忽略，可能主要会是空格或换行符。
        break;
    }
  }
  return 0;
}
